

#include "mapping/internal/2d/ray_casting.h"

#include <cstdint>
#include <cmath>

namespace cartographer {
namespace mapping {
namespace {

// Factor for subpixel accuracy of start and end point.
constexpr int kSubpixelScale = 1000;

// We divide each pixel in kSubpixelScale x kSubpixelScale subpixels. 'begin'
// and 'end' are coordinates at subpixel precision. We compute all pixels in
// which some part of the line segment connecting 'begin' and 'end' lies.
// 　真正的bresenham2d画线算法。这个函数被下面的CastRays调用。
//  CastRays里面的函数变量实际上是在这里面使用的。
void CastRay(const Eigen::Array2i &begin, const Eigen::Array2i &end, const std::vector<uint16_t> &miss_table,
             ProbabilityGrid *const probability_grid) {
    // For simplicity, we order 'begin' and 'end' by their x coordinate.
    if (begin.x() > end.x()) {
        CastRay(end, begin, miss_table, probability_grid);
        return;
    }

    // Special case: We have to draw a vertical line in full pixels, as 'begin'
    // and 'end' have the same full pixel x coordinate.
    if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) {
        Eigen::Array2i current(begin.x() / kSubpixelScale, std::min(begin.y(), end.y()) / kSubpixelScale);
        const int      end_y = std::max(begin.y(), end.y()) / kSubpixelScale;
        for (; current.y() <= end_y; ++current.y()) {
            probability_grid->ApplyLookupTable(current, miss_table);
        }
        return;
    }

    const int64_t dx          = end.x() - begin.x();
    const int64_t dy          = end.y() - begin.y();
    const int64_t denominator = 2 * kSubpixelScale * dx;

    // The current full pixel coordinates. We begin at 'begin'.
    Eigen::Array2i current = begin / kSubpixelScale;

    // To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in
    // the denominator.
    // +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale)
    // | | | |
    // +-+-+-+
    // | | | |
    // +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale)
    // | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale)
    // +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale)

    // The center of the subpixel part of 'begin.y()' assuming the
    // 'denominator', i.e., sub_y / denominator is in (0, 1).
    int64_t sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx;

    // The distance from the from 'begin' to the right pixel border, to be divided
    // by 2 * 'kSubpixelScale'.
    const int first_pixel = 2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1;
    // The same from the left pixel border to 'end'.
    const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1;

    // The full pixel x coordinate of 'end'.
    const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale;

    // Move from 'begin' to the next pixel border to the right.
    sub_y += dy * first_pixel;
    if (dy > 0) {
        while (true) {
            probability_grid->ApplyLookupTable(current, miss_table);
            while (sub_y > denominator) {
                sub_y -= denominator;
                ++current.y();
                probability_grid->ApplyLookupTable(current, miss_table);
            }
            ++current.x();
            if (sub_y == denominator) {
                sub_y -= denominator;
                ++current.y();
            }
            if (current.x() == end_x) {
                break;
            }
            // Move from one pixel border to the next.
            sub_y += dy * 2 * kSubpixelScale;
        }
        // Move from the pixel border on the right to 'end'.
        sub_y += dy * last_pixel;
        probability_grid->ApplyLookupTable(current, miss_table);
        while (sub_y > denominator) {
            sub_y -= denominator;
            ++current.y();
            probability_grid->ApplyLookupTable(current, miss_table);
        }
        // CHECK_NE(sub_y, denominator);
        // CHECK_EQ(current.y(), end.y() / kSubpixelScale);
        return;
    }

    // Same for lines non-ascending in y coordinates.
    while (true) {
        probability_grid->ApplyLookupTable(current, miss_table);
        while (sub_y < 0) {
            sub_y += denominator;
            --current.y();
            probability_grid->ApplyLookupTable(current, miss_table);
        }
        ++current.x();
        if (sub_y == 0) {
            sub_y += denominator;
            --current.y();
        }
        if (current.x() == end_x) {
            break;
        }
        sub_y += dy * 2 * kSubpixelScale;
    }
    sub_y += dy * last_pixel;
    probability_grid->ApplyLookupTable(current, miss_table);
    while (sub_y < 0) {
        sub_y += denominator;
        --current.y();
        probability_grid->ApplyLookupTable(current, miss_table);
    }
    // CHECK_NE(sub_y, 0);
    // CHECK_EQ(current.y(), end.y() / kSubpixelScale);
}

void GrowAsNeeded(const sensor::RangeData &range_data, ProbabilityGrid *const probability_grid) {
    Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
    constexpr float     kPadding = 1e-6f;
    for (const Eigen::Vector3f &hit : range_data.returns) {
        bounding_box.extend(hit.head<2>());
    }
    for (const Eigen::Vector3f &miss : range_data.misses) {
        bounding_box.extend(miss.head<2>());
    }
    probability_grid->GrowLimits(bounding_box.min() - kPadding * Eigen::Vector2f::Ones());
    probability_grid->GrowLimits(bounding_box.max() + kPadding * Eigen::Vector2f::Ones());
}

}  // namespace

// 进行栅格地图构建的时候需要用到的raytrace，基本算法为bresenham2d算法．
// 这个函数传入的参数 有两个是函数变量
// 这里面会调用真正的bresenham2d算法 CastRay()
// 这里面的函数成员变量hit_table 和 miss_table也是在castRay()中发挥作用的
void CastRays(const sensor::RangeData &range_data, const std::vector<uint16_t> &hit_table,
              const std::vector<uint16_t> &miss_table, const bool insert_free_space,
              ProbabilityGrid *const probability_grid) {
    GrowAsNeeded(range_data, probability_grid);

    const MapLimits     &limits                 = probability_grid->limits();
    const double         superscaled_resolution = limits.resolution() / kSubpixelScale;
    const MapLimits      superscaled_limits(superscaled_resolution, limits.max(),
                                            CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
                                                       limits.cell_limits().num_y_cells * kSubpixelScale));
    const Eigen::Array2i begin = superscaled_limits.GetCellIndex(range_data.origin.head<2>());

    // Compute and add the end points.
    // 计算激光雷达击中的坐标点
    std::vector<Eigen::Array2i> ends;
    ends.reserve(range_data.returns.size());
    for (const Eigen::Vector3f &hit : range_data.returns) {
        ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
        probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
    }

    if (!insert_free_space) {
        return;
    }

    // Now add the misses.
    // 计算激光雷达数据经过的空闲点
    for (const Eigen::Array2i &end : ends) {
        CastRay(begin, end, miss_table, probability_grid);
    }

    // Finally, compute and add empty rays based on misses in the range data.
    // 那些击中了空气的激光帧数据也会被用来计算freespace
    for (const Eigen::Vector3f &missing_echo : range_data.misses) {
        CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()), miss_table, probability_grid);
    }
}
}  // namespace mapping
}  // namespace cartographer
